Fusion of sensor and map data using constraint based optimization

ABSTRACT

Disclosed herein are methods and systems for fusion of sensor and map data using constraint based optimization. In an embodiment, a computer-implemented method may include obtaining tracking data for a tracked subject, the tracking data including data from a dead reckoning sensor; obtaining constraint data for the tracked subject; and using a convex optimization method based on the tracking data and the constraint data to obtain a navigation solution. The navigation solution may be a path and the method may further include propagating the constraint data by a motion model to produce error bounds that continue to constrain the path over time. The propagation of the constraint data may be limited by other sensor data and/or map structural data.

CROSS REFERENCE TO RELATED APPLICATIONS

This application is a continuation of U.S. patent application Ser. No.13/916,479, filed Jun. 12, 2013, which claims the benefit of U.S.Provisional Application No. 61/658,883, filed Jun. 12, 2012. Thecontents are hereby incorporated by reference in their entirety.

INCORPORATION BY REFERENCE

This application address concepts discussed in the followingpreviously-filed applications: “Mapping and Tracking System,” U.S.patent application Ser. No. 13/566,956, filed Aug. 3, 2012; “MethodsResolving the Elevation of a Tracked Personnel or Assets,” U.S.Provisional Patent Application Ser. No. 61/783,799, filed Mar. 14, 2013;“Methods for Improved Heading Estimation,” U.S. Provisional PatentApplication Ser. No. 61/783,908, filed Mar. 14, 2013; “Method to ScaleInertial Location Data Using Direction and/or Scale ConfidenceConstraints,” U.S. Provisional Patent Application Ser. No. 61/792,856,filed Mar. 15, 2013; “Method For Step Detection and GAIT DirectionEstimation,” U.S. patent application Ser. No. 13/827,506, filed Mar. 14,2013; and “System and Method for Localizing a Trackee at a Location andMapping the Location Using Inertial Sensor Formation,” U.S. patentapplication Ser. No. 13/852,649, filed Mar. 28, 2013. The aforementionedapplications in their entirety are incorporated by reference.

TECHNICAL FIELD

The technical field generally relates to a system and method forlocating, tracking, and/or monitoring the status of personnel or assets,both indoors and outdoors.

BACKGROUND

Derived relative motion information, which uses a dead reckoningprocess, is subject to cumulative error. Thus a tracking system relyingon dead reckoning alone may have a continuous decrease in accuracy,which makes derived relative motion information not trustworthy overlong periods of time. Many other aiding sensors have been considered,including ranging and optical based mapping systems.

The user track and map information that is acquired by use of multiplesensors, is combined so that the map information can compensate for deadreckoning, e.g., inertial drift while user motion/track information canallow perceptually aliased feature information to be disambiguated.Detected map features and ranges can feed into simultaneous localizationand mapping (SLAM) algorithms which provide corrections based on theinformation. The SLAM algorithms are typically implemented using aversion of Bayes filter such as a Kalman Filter.

SUMMARY

Disclosed herein are methods and systems for fusion of sensor and mapdata using constraint based optimization. In an embodiment, acomputer-implemented method may include obtaining tracking data for atracked subject, the tracking data including data from a dead reckoningsensor; obtaining constraint data for the tracked subject; and using aconvex optimization method based on the tracking data and the constraintdata to obtain a navigation solution.

In an embodiment a computing system used to track a trackee may includea dead reckoning sensor, a processor in communication with the deadreckoning sensor, and a memory coupled to the processor, the memoryhaving stored thereon executable instructions that when executed by theprocessor cause the processor to effectuate operations. The operationseffectuated may include obtaining tracking data for a tracked subject,the tracking data including data from the dead reckoning sensor,obtaining constraint data for the tracked subject, and using a convexoptimization method based on the tracking data and the constraint datato obtain a navigation solution.

This Summary is provided to introduce a selection of concepts in asimplified form that are further described below in the DetailedDescription. This Summary is not intended to identify key features oressential features of the claimed subject matter, nor is it intended tobe used to limit the scope of the claimed subject matter. Furthermore,the claimed subject matter is not limited to limitations that solve anyor all disadvantages noted in any part of this disclosure.

BRIEF DESCRIPTION OF THE DRAWINGS

A more detailed understanding may be had from the following description,given by way of example in conjunction with the accompanying drawingswherein:

FIG. 1 is an exemplary illustration of a series of poses from the deadreckoning path with features and constraints linked to poses;

FIG. 2 is an embodiment of a feature match corrected path of FIG. 1;

FIG. 3 is an exemplary block diagram illustrating how convex SLAM may beused in the context of tracking a device;

FIG. 4 is an exemplary method for acquiring a navigation solution orenforcing constraints in Convex SLAM;

FIG. 5 illustrates an error bound from a constraint, for example rangeor location, and that has been propagated by a motion velocity model;

FIG. 6 illustrates an error bounds propagated by a velocity model andpropagated using path based bounds on heading and scale;

FIG. 7 illustrates the minimized error bound of a velocity error boundpropagation and a path error bound propagation;

FIG. 8 is an exemplary illustration of a user with a tracking device asshown on a building floor plan;

FIG. 9 is an exemplary illustration of error propagation bounded by abuilding outline.

FIG. 10 is an exemplary illustration of error propagation bounded bywalls;

FIG. 11 shows an exemplary shape of the convex cost function where thecost is defined to be zero if the constraint is satisfied and can growoutside of that region; and

FIG. 12 is an exemplary block diagram representing a general purposecomputer system in which aspects of the methods and systems disclosedherein or portions thereof may be incorporated.

DETAILED DESCRIPTION

Inertial and other dead reckoning sensors are subject to drift and othererrors and these are accounted for in the design and operation ofnavigation methods. A significant problem associated with derivedrelative motion information, which uses a dead reckoning process, isthat it is subject to cumulative error. Thus a tracking system relyingon dead reckoning alone may have a continuous decrease in accuracy.Therefore, derived relative motion information may not be trustworthyover long periods of time. While improvements to sensor quality mayhelp, even high quality sensors have limitations in long durationtracking. Methods for error reduction over relatively longer periods aredisclosed.

Magnetic sensors, for example, are commonly used to aid inertial deadreckoning systems to improve heading. However, indoor environments andother environments where magnetic heading information is unreliable,present a problem.

Many other aiding sensors have been considered including ranging (e.g.,radio frequency (RF), acoustic, and LIDAR), optical, and inertial basedfeature based mapping systems, among other things. The benefit of theaforementioned aiding sensors is that their information is stationaryover time. Feature based systems may suffer from the perceptual aliasingproblem, e.g., for a given sensor system, two distinct places(features/landmarks) in the environment may appear the same.Consequently, in order to build reliable maps and allow long duration oftracking, the user track and map information is combined so that the mapinformation can compensate for dead reckoning systems error accumulationwhile user motion/track information from the dead reckoning system canallow perceptually aliased feature information to be disambiguated.

All of the detected map features and ranges can feed into simultaneouslocalization and mapping (SLAM) algorithms which provide correctionsbased on the information. An unbiased map is needed for localizationwhile an accurate pose estimate is needed to build that map. If at thenext iteration of map building the measured distance and directiontraveled has a budget of inaccuracies, driven by limited inherentprecision of the dead reckoning sensors and additional ambient noise,then any features being added to the map will contain correspondingerrors. Over time and motion, locating and mapping errors buildcumulatively, grossly distorting the map.

Because the position estimates and measurements are imperfect, thesolution to the SLAM problem requires the development of a way to updatethe uncertain geometric or topological environment model based on newobservations that maintained consistent interpretation of relationsbetween all of the uncertain features. The SLAM problem is typicallysolved using a Bayes filter implementation in a standard recursivetwo-step time-update (prediction), measurement-update form. Thederivation of this and similarly many other recursive state estimationfilters rely on the Markov assumption, which postulates that past andfuture data are independent given the current state. Because of concernsfor the practical implementation of Bayes filter at this level ofabstraction, approximations are often made to control computationalcomplexity, e.g. linearity of the state dynamics, Gaussian noise, etc.The resulting model inaccuracies may induce violations of the Markovassumption leading to a non-optimal solution. A Kalman filter andextended Kalman filters are special cases of a Bayesian filter.

An issue with standard implementations of Bayesian filtering techniquescurrently used in solving the SLAM problem are that they are notamenable to system reconfiguration. For example, standardimplementations often have fixed state vectors which limit the abilityto add or drop sensors. They also have significant performance issueswith respect to noise model mismatch. Many standard implementations areunable to handle non-Gaussian statistics or nonlinear measurement modelsin a near real time system.

Many aiding sensors of interest do not have Gaussian distribution, whichmay present a problem when using standard implementations of Bayesianfiltering techniques. For example, ranging sensors have non-Gaussiannoise in indoor environments where they experience significant multipathand are often in non-line of sight conditions. Optical feature detectionand ranging systems have error characteristics which vary nonlinearly asa function of distance.

To add to these issues, the problem of tracking and locating pedestrianspresents a set of challenges that is unique relative to most vehicletracking problems. Human walking has been studied for more than 50 yearsusing a variety of tools. While a variety human motion models areavailable, the inputs that drive those models are hard to identify.Additionally, the effectiveness of detailed human motion models isquestionable as the basis for nonlinear filtering and estimation methodsfor pedestrian tracking since the tracking system is typically based onmeasurements at one body location (on the waist or the foot are mostcommon) so has low effective observability. Because of these issues,routine application of standard filtering methods using these modelsdoes not provide the same type of benefits that they do in roboticsystems where known dynamic models and control inputs are the norm.

The aforementioned issues may be addressed by formulating the locationand mapping problem as a constraint based convex optimization problem inwhich the feature locations and ranges place distance constraints on thepath solution. The general approach in convex optimization is to definea convex objective function (i.e., cost) and to minimize the functionwhile enforcing the associated constraints on the function variables.Convex optimization may include efficient methods to solve for thevariables and the solution it yields is optimal with respect to thechosen objective function (i.e., cost).

In the convex optimization formulation, there are no assumptions made onerror model distributions. In addition there is no Markov assumption.The entire trajectory is maintained. In our implementation, discoveredfeatures are linked to the last consecutive pose (a pose is, forexample, inertial location and orientation of tracked subject) fromwhich the feature was seen and updated by default each time thehistorical path is updated. In this way the features do not have to beseparately tracked. FIG. 1 is an example. The feature distanceconstraints are iteratively enforced while minimizing the change in pathlength and path shape using distance and angle constraints,respectively.

Sensor ranging data and location data, e.g., GPS location, may behandled as constraints on the subject's track. For example, a rangingsensor provides a range to a known or estimated location and constrainsthe path to be within the provided range of the location/locationestimate; with a sensor that provides a location such as GPS, the pathis constrained to fall within the error bound of the given sensorlocation. Because sensors are handled as constraints, the algorithms arerobust to adding and dropping sensor data (as long as there are a set ofnavigation sensors available that can estimate a path). Also, since thehistorical data is kept, constraints may be imposed based on thehistorical data and the path corrected even if there is significantlatency. FIG. 2 is an exemplary illustration of a corrected path forFIG. 1. Examples of latency include latency communicating with a remoteprocessor, local processing delay (which could be elective due tolimited computing resources) or latency due to incomplete information.In the case of incomplete information, consider a tracked subject A mayget a range to tracked subject B, whose geo-referenced location isunknown when the range is taken; tracked subject B may later obtain datathat allows them to accurately geo-reference their location and that newinformation can be used to determine where B was when they ranged to A.Providing this updated information to tracked subject A, even with thedelay, may allow them to correct their historical path.

Maintaining the entire path history instead of only the most recent posehas several benefits including providing a directed approach foractively correcting the map and the entire path history when correctionsare indicated, for example when a feature is revisited. It also has thebenefit of enabling recovery from erroneous corrections. As disclosedherein, a SLAM method (for reference purposes, hereinafter “convexSLAM”) has been created to handle pedestrian navigation. Though thisSLAM method is also applicable to other platforms such as vehicles; invehicle problems known dynamic models and control inputs are the normand Bayesian Filtering methods have computationally efficient mechanismfor including this prior known information into the estimate in theprediction step which is why they tend to be favored.

FIG. 3 is an exemplary block diagram illustrating how convex SLAM may beused in the context of tracking a device (a trackee). In an embodiment,at block 305, appropriate local sensors (for example: accelerometer,gyroscope, GPS receiver, magnetic field sensor, barometric pressuresensor, or ranging sensors) are activated and data is received. At block310, sensor data is received and processed. In addition, features aredetected (for example, inertial sensors could detect a stairwell orelevator) and constraints are resolved. With regard to resolvedconstraints, the bounds in which the path must lie are determined basedon, for example, RF ranges, discovered features. Propagation of priorconstraints based on motion models and limits to error propagation suchas building information could also be completed. Also the convex costfunctions are set up.

At block 315, the navigation solution is updated. This could be doneusing convex optimization (FIG. 4 breaks this out in more detail, theoptimization steps 415 and 420 may include several iterations) or bysimply appending a corrected inertial path based on parameters from aprior iteration of the solution. Block 305, 310, and 315 usually happenin real-time, which is why a fast inertial update may be considered ifthere is a lot of data to process. These blocks may process all of thepath or only a small portion of the historical path in order to conservecomputational resources. If only a portion of the path is processed,some state information may be preserved to maintain the continuity ofthe prior solution knowledge. For example, what is called the path stateherein, the pose information (location and orientation), error bound ofthe break point and constraints on heading, drift, drift bounds, scaleand scale bounds from prior computations (e.g. from block 320). Theportion of the path to process can be determined, for example, based ona fixed processing time interval (e.g., 20 minutes), or adaptively, forexample based on path or location confidence. It may be desirable, forexample, to elect to break the path to save computation. The path may bebroken if there is a very good location/path estimate and it providestight constraints on the path state. At such time, accurately locatedfeatures may be stored in a global map where these features may not betied to a user path since the location error is small. The path may alsobe broken if the dead reckoning path confidence is very low for aperiod. At such time, the path may be broken and the system may attemptto globally reinitialize using other sensor data. For example, a trackeegets into a car and a pedometer system stops tracking, with GPS beingused to track the user in the interim, and the system reinitializing thedead reckoning path when the trackee gets out of the car.

Block 320, may process quality historical feature data and otherappropriate information that may be based on data from this trackingdevice or other tracking devices that have traversed the same path. Forexample, a stairwell previously visited by this tracking device or othertracking devices may be determined to match the stairwell the trackee iscurrently in, then prior information on the stairwell may be used. Whenquality historical features are returned, constraints are added thatconnect the new feature location to the prior feature location. Theportion of the path processed by these blocks is determined by the timeinterval since the feature was last seen. Block 320 runs navigationmethods detailed in FIG. 4. As shown in FIG. 3 by line 322, Block 320may send last pose, plus updated offset, rotation, scale, and driftparameters to Block 310. In an embodiment, Block 310 may apply it to theinertial path or to initialize optimization when a new constraint isprocessed. The constraint based optimization enforces the feature matchand the intermediate path is corrected, an example is shown in FIG. 1compared to FIG. 2.

Block 315 may pass along updated navigation information, which mayinclude pose or feature data (and the pose information (location andorientation), error bound of the first point and constraints on heading,drift, drift bounds, scale and scale bounds from prior computations) toblock 320 (convex SLAM) for further processing. Block 320 may receiveinformation from 315, historical data from previous processing that wasstored in pose and feature storage 325, and historical data associatedwith other tracking devices from a global coordinate manager 340. Block325 contains updated path (pose information) and related features. Theloop-closing detector 330 applies confidence/quality requirements beforeinformation is passed back to block 320 from the feature storage block325. At block 320, the information may be processed in near real time.At block 340, the global coordinate manager manages the disbursement ofupdated or corrected navigation information to the convex SLAM algorithmat block 320, and to or from other tracking devices which maycommunicate via network 335. Network 335 may be a wireless network andinclude a mesh network.

FIG. 1 is an exemplary illustration of an overview of how sensor datamay be processed when using a convex SLAM method during tracking. In anembodiment, block 305, block 310, and block 315 are usually thereal-time operations. The sensor data processing and feature detectionmay be embedded code on the sensor platform, which could be for example,a purpose built worn tracking unit or a cell phone with embeddednavigation sensors. The navigation algorithms receive sensor data andfeatures and provide an update in real time. Features and historicalpose data are passed to the convex SLAM algorithm which may operate in alower priority loop for near real time updates. Map management (andsharing of best quality features with other tracked subjects for sharingand asynchronously updating the global map) is provided (e.g. block340), as well as loop closure (e.g., block 330) detections of matchedfeatures and application of constraints to enforce the match when inputto the convex SLAM algorithm. The loop-closing detector 330 and GlobalCoordinate manager 340 may have different confidence/qualityrequirements (likely the Global Coordinate manager 340 will have tighterrequirements because trackee to trackee variation can cause matchingerrors whereas a single trackee path is more likely to have for example,consistent scale which helps when disambiguating features) beforeinformation is passed back to the convex SLAM 320.

FIG. 4 is an exemplary method 400 for acquiring a navigation solution(e.g., block 315) or enforcing the constraints in Convex SLAM (e.g.,block 320). An overview of the method that may be used in solving forthe navigation solution may include dead reckoning path acquisition at405, parameter initialization at block 410, global optimization at block415, local optimization at block 420, and ending with the final anavigation solution at block 425. The method 400 shows a simplifiedstructure and may have more complexity in the individual blocks, forexample initialization step may include determining certain parameterbounds before global optimization to minimize computational cost. Priorconstraints may be propagated based on motion models (for example, humanmax velocity) and propagation may be limited based on for example,building constraint information. Before any step a method may be used toeliminate constraints for example, because they are outliers or simplyto reduce computation when there are many constraints.

At block 405 there is acquisition of the dead reckoning path (herein forexample, we discuss an embodiment that takes as input an inertial path).A dead reckoning path may be created from any of a variety of methods,for example, inertial, optic flow, Doppler velocimeter, or somecombination of these. In one embodiment, a pedometer based method isused for computation of an inertial path.

At block 410, there is parameter initialization and boundsdetermination, which can be helpful to speed the solution (e.g., whencompass data is determined to be reliable). In an example, a compass maybe found to be reliable when it provides heading changes that match therate information returned from a gyro. After the compass is foundreliable, it can be used to place bounds on the possible headings and toprovide an initial drift estimate. New constraints based on for example,RF ranges, or discovered features are resolved to define the bounds inwhich the path must lie and the costs for violating those bounds, priorconstraints are propagated based on motion models and the propagation islimited based on for example, information on a building outline orinternal structure such as walls. Ranging and other path constraints mayalso be used to place bounds on heading and scaling. For example, aseries of GPS constraints along a portion of a path (where the errorbound received along with the GPS constraint is within a thresholdlevel) may be used to bound the scale and the heading. Parametersobtained from prior iterations of this algorithm may be used as theinitialization point when a new constraint is added.

At block 415, there is global optimization. This involves thecomputation of a globally optimal solution based on the use of convexoptimization techniques and a defined convex cost function forparameters offset (initial location), scale, rotation, and drift givenconstraints from ranging, GPS, discovered map features, usercorrections, check-ins, etc. This is described in more detail below.

At block 420, there is local optimization. For polygon or circleconstraints that are not met, computation of a local modification of thepath can be made between constraints to satisfy these remainingconstraints using a physics-based constraint solver. These are standardparticle solvers used in computer graphics engines where the lengthconstraints between particles (or path points) can be thought of aslinear springs and the constraints on angles can be thought of astorsional springs. In this way the path is similar to a rubber band thatis stretched to satisfy the constraints. This is described in moredetail below.

With regard to local optimization, in an embodiment an inertial path isallowed to stretch or shrink in length between points with the costincreasing as a function of the change in length (linear springconstant) enforcing the goal of maintaining the length as close aspossible to the original length. The angles between path points aretightly maintained (very high torsional spring constant). Thisrestriction is based on the knowledge that the gyro heading is has lowerror over the short term so these heading angles should be essentiallycorrect and therefore not distorted. The local optimization can beterminated when the constraints are all solved or when it reaches alimit on the number of iterations.

Before the local optimization is completed, a median (or other) filtermay be used for removal of the outliers caused by sensor errors. If x isthe amount by which the constraint is not satisfied, the median absolutedeviation (MAD) is defined as the median of the absolute deviations fromthe median:

${{MAD}(X)} = {\underset{x \in X}{median}\left( {{x - {{median}(X)}}} \right)}$

As a rule of thumb, the median and MAD are typically not affected byoutliers unless more than 50% of the data are outliers, whereas the meanand standard deviation may be affected by a single outlier. The filterexcludes features outside some fixed number of MADs from the median.Theoretically, with any nicely distributed data, a single MAD away fromthe median in either direction will bound 50% of the data. With aGaussian distribution, two MADs will bound about 82% of the data. ThreeMADs are nearly equivalent to two standard deviations under a Gaussiandistribution, bounding about 95% of the data. In this embodiment, pointswithin 1 MAD are considered.

Ranging, GPS, given and discovered map features (e.g. stairwells,elevators, and hallway intersections), user corrections, check-ins, andother sensor and map information all place constraints on the path thatare defined by a circle or polygon. The center of the circle or polygonis at the estimated location of the feature, range origin, or the likeand has a radius defined by the error bound. Other map features such ashallways and walls provide a bound on the propagation of constraintsover time that is described in more detail below.

While there are no constraints on the types of error models, sensorerror distribution can significantly contribute to a good navigationsolution. The goal is to have a bound that encompasses as close to 100%of the points as possible. This is not practical, which is why a step ininserted to remove outliers before enforcing all constraints in thelocal optimization step.

The error circles from various sensors may be combined to form polygons.This geometric representation allows intersections to be efficientlycomputed. The bounds created are a mechanism for constraining locationerror. This error bound may be combined with inertial path data toconstrain the path.

For ranging data, the estimated or known location of the ranging sourcedefines the circle center and the constraint is obtained by adding theuncertainty in the location to the received range measurement. Here, itis assumed that ranging measurements are greater than the actualmeasurement and not less. This is not true for all ranging sensors.

GPS error bounds reported by sensors typically give an error standarddeviation measure that indicates 67% of the time, it is expected that atracked device is within the error bound relative to the reportedlocation. Systems may also report a circular error probable (CEP) whichgives a bound for only 50% of the time. In an embodiment, three timesthe error bound may be used, which relies on the three-sigma rule whichstates that nearly all values lie within 3 standard deviations of themean in a Gaussian distributed data set. GPS does not have a Gaussiandistribution in urban areas or near buildings.

These constraints may have error bounds defined by a user when thecheck-in are created. In another embodiment, the constraint error boundsmay be defined automatically based on the zoom level of the map in orderto make corrections when the location of the user is indicated (e.g.,selected on a map).

In pre-mapped buildings, the error bound may be defined by the size ofthe building feature and the accuracy with which the building featurelocation is known. If the building feature is discovered by thenavigation system, then the error bound is a function of a locationestimate of the person at the time the building feature was discovered.

The error circles from various sensors may be combined to form polygons.Error bounds may be propagated based on a model of human motion. Forexample, computing the maximum distance a user could have traveled sincethe range was received based on the assumption of a maximum velocitycould be used to propagate the error bound in all directions. Anotherpossible method would include combining human motion model with measuredsensor data, for example, using a combination of the time elapsedassuming a maximum velocity and the computed distance traversed based onthe inertial sensors. This bound would take into account the heading andscaling bounds similar to what is shown in FIG. 5. For example, in FIG.5, a tracked device may have an estimated initial point at circle 505with an estimated velocity error bound by circle 510. In FIG. 6,additional information regarding the estimated inertial path error boundby shape 515. In FIG. 7, considering velocity error bound by circle 510and inertial path bound by shape 515, the minimized error for thelocation of the tracked device is bound by shape 520.

The size of the bounds can be reduced, if given additional informationduring the propagation step. For example, if it is known that a user ofa mobile device is indoors (this can be determined using sensor data),the error may be bound based on the building outline. FIG. 8 is anexemplary illustration of a user with a tracked mobile device and anassociated building floor plan. In FIG. 8, a tracked mobile devicecarried by a user has an estimated location of 805 and error circle of810. The error circle 810 stretches beyond the indoor walls and theoutline wall 812 that marks the outside of building 800. In FIG. 9, atracked mobile device carried by a user has an estimated location of 905and error boundary of 910. The error boundary 910 stretches beyond theindoor walls, but does not stretch beyond the outline wall 912 thatmarks the outside of building 900.

In the examples shown in FIG. 8 and FIG. 9, there results in an errorbound that is the intersection of a circle as well as a buildingpolygon. In implementing this for some building outlines, the errorbound computed remains correct, but it may not be convex. In order to beassured of a globally optimal solution, in step 415 of FIG. 4, theconstraints must be convex. By computing a convex hull, it is possibleto use the building constraints to obtain a more precise bound, but thesolution may not respect the walls that fall within the convex hull ofthe propagated bounds. In step 420 of FIG. 4, convex polygons are notrequired, so the local optimization will correct the solution to remainin the bounds.

In FIG. 10, a tracked mobile device carried by a user has an estimatedlocation of 1005 and shaded error boundary of 1010. The error boundary1010 does not stretch beyond the outline wall 1012 that marks theoutside of building 1000. In addition, the error boundary 1010 does notstretch over or through the indoor walls 1014, 1016, and 1018. There isa convex flood area 1020 that passes around the walls and into theadjacent room and hall. A building can be a relatively large area, butusing walls to restrict the propagation, forces a more complexrepresentation of error bounds. In step 415 of FIG. 4, the constraintsmust be convex. By computing a convex hull for this step, it is possibleto use the building constraints to obtain a more precise bound, but thesolution may not respect the walls that fall within the convex hull ofthe propagated bounds. In step 420 of FIG. 4, convex polygons are notrequired so the local optimization will correct the solution to enforcethat it remains in the computed error bounds shown in FIG. 10.

The proposed approach for bounding error by walls and using a convexhull, as illustrated in FIG. 10, allows new information to be ingestedby a navigation system. While the necessary propagation may be performeddirectly on raster floor-plan images, such partial flood fillingoperation may not be performed efficiently. A more efficient mechanismfor propagating the bounds may include a geometric representation of thebuilding structure. The walls of the building may be represented as linesegments and the intersections. Because the walls are uniformlypresented as segments we are able to stored them in a standard linkeddata structure in a way that allows for quick traversal between edgesand vertices due to the explicitly linked structure of the data storage.

In order to perform partial flood filling, as shown in FIG. 10, there isan operation that allows a polygon to be constructed that is visiblefrom any point in the building. In addition, there is an operation thatallows walls to be clipped away within the original ranging distancewhen performing the method. Both of these operations may be doneapproximately and somewhat inefficiently using a KD-Tree, QuadTree, orBSP Tree.

Below is one embodiment of a function that turns a range into an errorbound that takes walls into account. A range is defined as Center,Radius, Propagated Radius (C, R, PR), where the ranges are propagatedsimilarly as discussed herein.

ExpandBoundsWithWalls(C, R, PR)

{ If (PR == 0) return EmptyPolygon Polygon visible =FindVisiblePolygon(C, R) //Cacheable for R == 0 Polygon inRange =intersection(visible, Circle(C, PR)) foreach Vertex V in visible {Polygon result = ExpandBoundsWithWalls(V, 0, (PR − (dist(C, V))) inRange= Union(inRange, result) } return inRange }

The global optimization method includes computing the distance betweendead reckoning path point and constraint for each point/constraint pair.The cost function is defined to be zero when the constraints are met,and grow as a function of the distance from the constraint, in theembodiment described herein, distance squared is used. FIG. 11 shows anexemplary shape of the convex cost function where the cost is defined tobe zero if the constraint is satisfied and can grow outside of thatregion.

There are multiple methods available for solving convex optimizationproblems. In an embodiment, the optimization may be solved using anadaptive gradient descent method until the cost is zero, stopsdecreasing, or a max number of iterations have been completed. Theparameters may be solved all at once or in a particular order, forexample 1—translation, 2—rotation; and 3—scale.

In the aforementioned embodiment, each time cost is non-zero, aderivative is computed with respect to each of four parameters: xtranslation, y translation, rotation, and scale. In an embodiment 2-D(x,y) offset can be solved and the elevation (z) offset may be solvedfor separately. Solving for the 3-D offset would be an extension of thismethod. See below for details of the 2-D derivations.

Each parameter may be updated by weighted sum over all constraints ofthe derivatives for the respective parameter. The weights may reflectthe adaptive step scale value or a confidence or priority measure on theconstraint. For example, constraints created by GPS measurements mayhave a different weighting than user corrections. The derivativescalculation can be repeated until cost is zero, cost stops decreasing,or 1000 iterations, for example.

The drift is a nonlinear parameter. The heading drift if statedtypically in degrees (or radians) per second. The drift may be applied(or removed) from path data by rotating the path over each time step bythe drift over that time step. In this embodiment, the optimal drift isselected by computing the cost for a fixed number of drift values withinthe drift bounds set in the parameter initialization step. The minimumacross these computed drift values is selected. If two drift values havethe same cost, the drift with the least magnitude is selected. In thisembodiment, 51 was selected as the fixed number of drift values togenerally give adequate heading refinement from the compass initializeddrift value which can be very noisy in buildings.

The path point, related constraint center or nearest constraint polygonpoint is as denoted in Table 3.

TABLE 1 path point P = (P_(x), P_(y)) related constraint circle center C= (C_(x), C_(y)) nearest point on constraint polygon N = (N_(x), N_(y))The parameters for translation, rotation and scale are denoted in Table4.

TABLE 2 translation T = (T_(x), T_(y)) rotation θ scale SThe parameters modify the path by the following equation:

${Path} = {{{S\begin{bmatrix}{\cos (\theta)} & {\sin (\theta)} \\{{- \sin}\; (\theta)} & {\cos (\theta)}\end{bmatrix}}P} + {T.}}$

The cost and related derivatives for each polygon and circle constraintused in the gradient decent algorithms are shown in Table 3.

TABLE 3 Convex Polygon Circle Cost = ||P − N||² Cost = (||P − C|| −MaxDist)²$\frac{\partial{Cost}}{\partial T_{x}} = {2\left( {P_{x} - N_{x}} \right)}$$\frac{\partial{Cost}}{\partial T_{x}} = {\left( {2 - \frac{2{MaxDist}}{\left. ||{P - C} \right.||}} \right)\left( {P_{x} - C_{x}} \right)}$$\frac{\partial{Cost}}{\partial T_{y}} = {2\left( {P_{y} - N_{y}} \right)}$$\frac{\partial{Cost}}{\partial T_{y}} = {\left( {2 - \frac{2{MaxDist}}{\left. ||{P - C} \right.||}} \right)\left( {P_{y} - C_{y}} \right)}$$\frac{\partial{Cost}}{\partial\theta} = {{{- 2}\left( {P_{y} - T_{y}} \right)\left( {P_{x} - N_{x}} \right)} + {2\left( {P_{x} - T_{x}} \right)\left( {P_{y} - N_{y}} \right)}}$$\frac{\partial{Cost}}{\partial\theta} = {\left( {{{- 2}\left( {P_{y} - T_{y}} \right)\left( {P_{x} - C_{x}} \right)} + {2\left( {P_{x} - T_{x}} \right)\left( {P_{y} - C_{y}} \right)}} \right)\left( \frac{\left. ||{P - C}||{- {MaxDist}} \right.}{\left. ||{P - C} \right.||} \right)}$$\frac{\partial{Cost}}{\partial S} = \frac{{2\left( {P_{x} - T_{x}} \right)\left( {P_{x} - N_{x}} \right)} + {2\left( {P_{y} - T_{y}} \right)\left( {P_{y} - N_{y}} \right)}}{S}$$\frac{\partial{Cost}}{\partial S} = {\left( \frac{{2\left( {P_{x} - T_{x}} \right)\left( {P_{x} - C_{x}} \right)} + {2\left( {P_{y} - T_{y}} \right)\left( {P_{y} - C_{y}} \right)}}{S} \right)\left( \frac{\left. ||{P - C}||{- {MaxDist}} \right.}{\left. ||{P - C} \right.||} \right)}$

As the number of path constraints grow, so does the computationalcomplexity. Multiple methods may be created to eliminate constraintsthat do not provide information or provide marginal information. In anembodiment, this can be done by eliminating constraints that do notcontribute to the final error bound polygon at a particular point. Forexample, if there is a constraint of 1 meter from a ranging anchor and aGPS constraint of 10 meters at the same point, the GPS constraintbecomes irrelevant. In another example, you may receive a GPS constraintonce per second outside. If GPS is good, e.g., as it should be in afield, then you could walk the length of the field receiving a 2 meterconstraint every second. Assuming the inertial path is good (you are notmissing steps.), it is likely “good enough” to have a constraint at eachend of the field. It is not necessary to keep using all the constraints,but just the first and last as the tracked subject moved along.

A tracking device may include a ranging sensor. RF ranging sensors havehighly non-Gaussian properties in harsh multipath environments whichfrustrates integration into filters that have Gaussian assumptions onerror distributions. The constraint based optimization solutiondescribed herein does not make assumptions on the expected errordistribution of the sensors. Instead it uses ranging sensors to imposeconstraints on the tracked subject's trajectory. Ranging constraintsfrom single or multiple sources may provide frequent input to limit thesolution space. Even tracking devices that are close together mayreceive varying ranging measurements due to multipath and signalblockage from others in the vicinity, the body of the person using thetracking device, or other carried equipment. This type of ranging errormay be difficult to predict.

The disclosed constraint based optimization solution is able to supportcollaborative navigation based on real-time ranging corrections (personto person, person to anchor/base station) using embedded chirp spreadspectrum time of flight ranging (e.g., short range of approximately 50meters) to significantly improve tracking performance. Ranging to fixedanchors and person to person (i.e., tracking device to tracking device)is used for automatic initialization (as an alternative to GPS) when atleast one global location is known. Person to person ranges providecontinuous navigation corrections to keep team members together andimprove the overall navigation solution and provide relative (person toperson) location accuracy that usually does not degrades over time.

A test course was traversed in a multi-story office building of steeland concrete construction with the system discovering map features asusers walked, rode elevators, and traversed stairs—with no assistancefrom preinstalled infrastructure. Over several trials the trackedlocations were on average accurate to within 3.5 meters throughout theentire course. The path, track, and elevation provided locationinformation for personnel being tracked, and the system visually showedthe location and tracked in 3 dimensional space.

Another test course was traversed in an underground test facility wherethere were at least 5 persons being tracked throughout the facility. Amesh communications network was set up using radios at multiple points.Navigation system initialization was performed automatically using acombination of GPS, which was available outside of the structure andranging anchors, which where were placed at three known locationsoutside. Ranging anchors were “dropped” as part of the mission scenarioat each of three key network points. The locations of interior pointswere unknown to the navigation system so the dropped ranging anchorlocations were estimated by the navigation system.

Without in any way limiting the scope, interpretation, or application ofthe claims appearing herein, a technical effect of one or more of theexample embodiments disclosed herein is to provide adjustments tonavigation solutions based on using convex optimization.

The techniques described above can be implemented on a computing deviceassociated with a user (e.g., gyroscope and accelerometer sensorsimplemented on a device worn by the user), a plurality of computingdevices associated with a plurality of users, a server in communicationwith the computing device(s) (e.g., a server configured to calibrate thegyroscope and accelerometer sensors of the device worn by the user), ora plurality of servers in communication with the computing device(s).Additionally, the techniques may be distributed between the computingdevice(s) and the server(s). For example, the computing device maycollect and transmit raw data to the server that, in turn, process theraw data to improve heading estimation. FIG. 12 illustrates an exemplaryblock diagram of a computing system that includes hardware modules,software module, and a combination thereof and that can be implementedas the computing device and/or as the server.

In a basic configuration, the computing system may include at least aprocessor, a system memory, a storage device, input/output peripherals,communication peripherals, and an interface bus. The interface bus isconfigured to communicate, transmit, and transfer data, controls, andcommands between the various components of the electronic device. Thesystem memory and the storage device comprise computer readable storagemedia, such as RAM, ROM, EEPROM, hard-drives, CD-ROMs, optical storagedevices, magnetic storage devices, flash memory, and other tangiblestorage media. Any of such computer readable storage medium can beconfigured to store instructions or program codes embodying aspects ofthe disclosure. Additionally, the system memory comprises an operationsystem and applications. The processor is configured to execute thestored instructions and can comprise, for example, a logical processingunit, a microprocessor, a digital signal processor, and the like.

The system memory and the storage device may also comprise computerreadable signal media. A computer readable signal medium may include apropagated data signal with computer readable program code embodiedtherein. Such a propagated signal may take any of variety of formsincluding, but not limited to, electro-magnetic, optical, or anycombination thereof. A computer readable signal medium may be anycomputer readable medium that is not a computer readable storage mediumand that can communicate, propagate, or transport a program for use inconnection with the computing system.

Further, the input and output peripherals include user interfaces suchas a keyboard, screen, microphone, speaker, other input/output devices,and computing components such as digital-to-analog and analog-to-digitalconverters, graphical processing units, serial ports, parallel ports,and universal serial bus. The input/output peripherals may be connectedto the processor through any of the ports coupled to the interface bus.

The user interfaces can be configured to allow a user of the computingsystem to interact with the computing system. For example, the computingsystem may include instructions that, when executed, cause the computingsystem to generate a user interface that the user can use to provideinput to the computing system and to receive an output from thecomputing system.

This user interface may be in the form of a graphical user interfacethat is rendered at the screen and that is coupled with audiotransmitted on the speaker and microphone and input received at thekeyboard. In an embodiment, the user interface can be locally generatedat the computing system. In another embodiment, the user interface maybe hosted on a remote computing system and rendered at the computingsystem. For example, the server may generate the user interface and maytransmit information related thereto to the computing device that, inturn, renders the user interface to the user. The computing device may,for example, execute a browser or an application that exposes anapplication program interface (API) at the server to access the userinterface hosted on the server.

Finally, the communication peripherals of the computing system areconfigured to facilitate communication between the computing system andother computing systems (e.g., between the computing device and theserver) over a communications network. The communication peripheralsinclude, for example, a network interface controller, modem, variousmodulators/demodulators and encoders/decoders, wireless and wiredinterface cards, antenna, and the like.

The communication network includes a network of any type that issuitable for providing communications between the computing device andthe server and may comprise a combination of discrete networks which mayuse different technologies. For example, the communications networkincludes a cellular network, a WiFi/broadband network, a local areanetwork (LAN), a wide area network (WAN), a telephony network, afiber-optic network, or combinations thereof. In an example embodiment,the communication network includes the Internet and any networks adaptedto communicate with the Internet. The communications network may be alsoconfigured as a means for transmitting data between the computing deviceand the server.

The techniques described above may be embodied in, and fully orpartially automated by, code modules executed by one or more computersor computer processors. The code modules may be stored on any type ofnon-transitory computer-readable medium or computer storage device, suchas hard drives, solid state memory, optical disc, and/or the like. Theprocesses and algorithms may be implemented partially or wholly inapplication-specific circuitry. The results of the disclosed processesand process steps may be stored, persistently or otherwise, in any typeof non-transitory computer storage such as, e.g., volatile ornon-volatile storage.

The various features and processes described above may be usedindependently of one another, or may be combined in various ways. Allpossible combinations and sub-combinations are intended to fall withinthe scope of this disclosure. In addition, certain method or processblocks may be omitted in some implementations. The methods and processesdescribed herein are also not limited to any particular sequence, andthe blocks or states relating thereto can be performed in othersequences that are appropriate. For example, described blocks or statesmay be performed in an order other than that specifically disclosed, ormultiple blocks or states may be combined in a single block or state.The example blocks or states may be performed in serial, in parallel, orin some other manner. Blocks or states may be added to or removed fromthe disclosed example embodiments. The example systems and componentsdescribed herein may be configured differently than described. Forexample, elements may be added to, removed from, or rearranged comparedto the disclosed example embodiments.

Conditional language used herein, such as, among others, “can,” “could,”“might,” “may,” “e.g.,” and the like, unless specifically statedotherwise, or otherwise understood within the context as used, isgenerally intended to convey that certain embodiments include, whileother embodiments do not include, certain features, elements, and/orsteps. Thus, such conditional language is not generally intended toimply that features, elements and/or steps are in any way required forone or more embodiments or that one or more embodiments necessarilyinclude logic for deciding, with or without author input or prompting,whether these features, elements and/or steps are included or are to beperformed in any particular embodiment. The terms “comprising,”“including,” “having,” and the like are synonymous and are usedinclusively, in an open-ended fashion, and do not exclude additionalelements, features, acts, operations, and so forth. Also, the term “or”is used in its inclusive sense (and not in its exclusive sense) so thatwhen used, for example, to connect a list of elements, the term “or”means one, some, or all of the elements in the list.

While certain example embodiments have been described, these embodimentshave been presented by way of example only, and are not intended tolimit the scope of the inventions disclosed herein. Thus, nothing in theforegoing description is intended to imply that any particular feature,characteristic, step, module, or block is necessary or indispensable.Indeed, the novel methods and systems described herein may be embodiedin a variety of other forms; furthermore, various omissions,substitutions and changes in the form of the methods and systemsdescribed herein may be made without departing from the spirit of theinventions disclosed herein. The accompanying claims and theirequivalents are intended to cover such forms or modifications as wouldfall within the scope and spirit of certain of the inventions disclosedherein.

Real-time as discussed herein refers to operations that usually occur inmilliseconds, but not more than one second. The term near real-timerefers to the time delay introduced by automated data processing ornetwork transmission, between the occurrence of an event and the use ofthe processed data, such as for display or feedback and controlpurposes. For example, a near-real-time display depicts an event orsituation as it existed at the current time minus the processing time,as nearly the time of the live event. Near real-time events usuallyoccur within seconds.

What is claimed:
 1. A computer-implemented method of simultaneouslytracking a tracked device and determining a location of features,comprising: obtaining sensor data from the tracked device, the sensordata including location data and dead reckoning sensor data; determiningdead reckoning path data based on the sensor data; detecting one or morefeatures based on the sensor data; detecting a location for each featurebased on the dead reckoning path data; determining at least oneconstraint based on the sensor data; and optimizing the dead reckoningpath data and updating the location of each feature using a convexsimultaneous location and mapping algorithm comprising: defining aconvex objective function for the dead reckoning path data; minimizingthe convex objective function based on the at least one constraint;applying the convex objective function to update the dead reckoning pathdata; and updating the location of each feature based on the updateddead reckoning path data.
 2. The method of claim 1, wherein the sensordata further comprises one or more of inertial sensor data, gyroscopedata, magnetic sensor data, barometric pressure sensor data, and rangingdata.
 3. The method of claim 1, wherein the dead reckoning path datacomprises one or more of an offset, a rotation, a scale, and a drift forone or more path segments.
 4. The method of claim 1, wherein the one ormore features include one or more of a stairwell, an elevator, ahallway, and a building feature.
 5. The method of claim 1, wherein theat least one constraint is based on historical data.
 6. The method ofclaim 5, wherein the historical data comprises one or more of apreviously detected feature, dead reckoning path data from the trackeddevice, dead reckoning path data from another tracked device, and aprior constraint.
 7. The method of claim 1, wherein the at least oneconstraint is one or more of an error bound on a location, a drift, aheading, and a scale of one or more segments of the dead reckoning pathdata.
 8. The method of claim 7, wherein a location error bound is basedon one or more of a radiofrequency (RF) range, a GPS constraint, adetected feature, and a prior constraint.
 9. The method of claim 1,wherein optimizing the dead reckoning path data further comprisesremoving an outlier in the sensor data.
 10. The method of claim 9,wherein the outlier is detected by a medium absolute deviation from theat least one constraint.
 11. The method of claim 1, wherein optimizingthe dead reckoning path data further comprises removing the at least oneconstraint based on one or more of an outlier in the sensor data, asensor error, and a computational performance measure.
 12. The method ofclaim 1, wherein the at least one constraint is iteratively enforcedwhen there is more than one constraint.
 13. A computer readable storagemedium comprising instructions that, when executed on a computing systemto simultaneously track a tracked device and determine a location offeatures, cause the computing system to at least: obtain sensor datafrom the tracked device, the sensor data comprising location data anddead reckoning sensor data; determine dead reckoning path data based onthe sensor data; detect one or more features based on the sensor datadetect a location for each feature based on the dead reckoning pathdata; determine at least one constraint based on the sensor data; andoptimize the dead reckoning path data and update the location of eachfeature using a convex simultaneous location and mapping algorithmcomprising instructions that further cause the computing system to atleast: define a convex objective function for the dead reckoning pathdata; minimize the convex objective function based on the at least oneconstraint; apply the convex objective function to update the deadreckoning path data; and update the location of each feature based onthe updated dead reckoning path data.
 14. The computer readable storagemedium of claim 13, further comprising instructions that cause thecomputing system to at least: store, in memory, the optimized deadreckoning path data, including the at least one constraint and one ormore features, for use in a subsequent convex simultaneous location andmapping algorithm.
 15. The computer readable storage medium of claim 13,wherein the sensor data further comprises one or more of inertial sensordata, gyroscope data, magnetic sensor data, barometric pressure sensordata, and ranging data.
 16. The computer readable storage medium ofclaim 13, wherein the dead reckoning path data comprises one or more ofan offset, a rotation, a scale, and a drift for one or more pathsegments.
 17. The computer readable storage medium of claim 13, whereinthe at least one constraint is based on historical data comprising oneor more of a previously detected feature, dead reckoning path data fromthe tracked device, dead reckoning path data from another trackeddevice, and a prior constraint.
 18. The computer readable storage mediumof claim 17, wherein the at least one constraint is an error bound on alocation, a drift, a heading, and a scale of one or more segments of thedead reckoning path data.
 19. The computer readable storage medium ofclaim 18, wherein the location error bound is based on one or more of aradiofrequency (RF) range, a GPS constraint, a detected feature, and aprior constraint.
 20. The computer readable storage medium of claim 13,wherein the instructions that optimize the dead reckoning path datafurther cause the computing system to at least: remove an outlier in thesensor data.
 21. The computer readable storage medium of claim 20,wherein the outlier is detected by a medium absolute deviation from theat least one constraint.
 22. The computer readable storage medium ofclaim 13, wherein the instructions that optimize the dead reckoning pathdata further cause the computing system to at least: remove the at leastone constraint based on one or more of an outlier in the sensor data, asensor error, and a computational performance measure.
 23. The computerreadable storage medium of claim 13, wherein the instructions thatoptimize the dead reckoning path data further cause the computing systemto at least: apply a particle solver to resolve the at least oneconstraint.
 24. A computing system for simultaneously tracking a trackeddevice and determining a location of features comprising: a processor; amemory communicatively coupled to the processor, the memory bearinginstructions that, when executed on the processor, cause the computingsystem to at least: obtain sensor data from the tracked device, thesensor data comprising location data and dead reckoning sensor data;determine dead reckoning path data based on the sensor data; detect oneor more features based on the sensor data; detect a location for eachfeature based on the dead reckoning path data; determine at least oneconstraint based on the sensor data; and optimize the dead reckoningpath data and update the location of each feature using a convexsimultaneous location and mapping algorithm comprising instructions thatfurther cause the computing system to: define a convex objectivefunction for the dead reckoning path data; minimize the convex objectivefunction based on the at least one constraint; apply the convexobjective function to update the dead reckoning path data; and updatethe location of each feature based on the updated dead reckoning pathdata.
 25. The computing system of claim 24, wherein the sensor datafurther comprises one or more of inertial sensor data, gyroscope data,magnetic sensor data, barometric pressure sensor data, and ranging data.26. The computing system of claim 24, wherein the dead reckoning pathdata comprises one or more of an offset, a rotation, a scale, and adrift for one or more path segments.
 27. The computing system of claim24, wherein the at least one constraint is based on historical datacomprising one or more of a previously detected feature, dead reckoningpath data from the tracked device, dead reckoning path data from anothertracked device, and a prior constraint.
 28. The computing system ofclaim 27, wherein the at least one constraint is one or more of an errorbound on a location, a drift, a heading, and a scale of one or moresegments of the dead reckoning path data.
 29. The computing system ofclaim 28, wherein the location error bound is based on one or more of aradiofrequency (RF) range, a GPS constraint, a detected feature, and aprior constraint.
 30. The computing system of claim 24, wherein theinstructions that optimize the dead reckoning path data, when executedon the processor, further cause the computing system to at least: removean outlier in the sensor data.
 31. The computing system of claim 24,wherein the instructions that remove the at least one constraint isbased on one or more of an outlier in the sensor data, a sensor error,and a computational performance measure.